#include "StdAfx.h"
#include "ToroidalMillingTool.h"
#include <math.h>


ToroidalMillingTool::ToroidalMillingTool(int diameter,double length):MillingTool(length,diameter)
{
	this->diameter = diameter;
}

ToroidalMillingTool::~ToroidalMillingTool(void)
{
}

double ToroidalMillingTool::GetMillingErrorAt(const Point3D & surfacePoint)
{
	return 0.0;
}

bool ToroidalMillingTool::surpassMovementDirectionCond(Point3D &p,const hash_set<int> * visitedPoints)
{
	int code = p.getHashCode2D();
	std::hash_set<int>::iterator it = visitedPoints->find(p.getHashCode2D());
	if(it!=visitedPoints->end())
		return false;
	if(p.x<(MY_DATATYPE)(this->posX - this->diameter/2) || p.x > (MY_DATATYPE)(this->posX + this->diameter/2))
		return false;
	if(p.y<(MY_DATATYPE)(this->posY - this->diameter/2) || p.y > (MY_DATATYPE)(this->posY + this->diameter/2))
		return false;

	if(pow((double)p.x-this->posX,2) + pow((double)p.y-this->posY,2)>pow((double)this->diameter/2.0,2))
		return false;
	
	Vector4D vp(p.x - (MY_DATATYPE) this->posX,p.y - (MY_DATATYPE) this->posY,p.z - (MY_DATATYPE) this->posZ);
	if(abs(vp.x - 0.0)>EPSILON || abs(vp.y - 0.0)>EPSILON || abs(vp.z - 0.0)>EPSILON)
	vp = Vector4D::normalize(vp);
	if(Vector4D::dotProduct(vp,this->direction)<0.0)
		return false;

	return true;
}

void ToroidalMillingTool::Draw()
{
}